import math
import time
from typing import List

GRAVITY = 9.80665
CONSTANTS_RADISU_OF_EARTH = 6371000
EARTH_RADIUS = 6378137

def constrain(val, min_val, max_val):
    return max(min(val, max_val), min_val)

def abs_num(x):
    if x < 0:
        return -x
    else:
        return x

def ISFINITE(x):
    if abs_num(x) > 0.02 and abs_num(x) < 1000:
        return True
    else:
        return False
    
def max(a, b):
    if a > b:
        return a
    else:
        return b
    
def min(a, b):
    if a < b:
        return a
    else:
        return b
    
def quat_2_euler(quat: List[float]) -> List[float]:
    """
    @Description : 四元数转欧拉角
    @Param: 
        quat: 四元数 
        q0 q1 q2 q3
        w x y z
    @Returns     :
        quler: 欧拉角
        roll pitch yaw
    """
    angle = [0.0, 0.0, 0.0]
    angle[0] = math.atan2(2 * (quat[0] * quat[1] + quat[2] * quat[3]), 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]))
    angle[1] = math.asin(constrain(2 * (quat[0] * quat[2] - quat[3] * quat[1]), -1, 1))
    angle[2] = math.atan2(2 * (quat[0] * quat[3] + quat[1] * quat[2]), 1 - 2 * (quat[2] * quat[2] + quat[3] * quat[3]))

    return angle

def euler_2_quat(euler: List[float]) -> List[float]:
    """
    @Description : 欧拉角转四元数
    @Param:
        euler: 欧拉角
        roll pitch yaw
    @Returns     :
        quat: 四元数
        q0 q1 q2 q3
        w x y z
    """
    cosPhi_2 = math.cos(euler[0] / 2)
    sinPhi_2 = math.sin(euler[0] / 2)
    cosTheta_2 = math.cos(euler[1] / 2)
    sinTheta_2 = math.sin(euler[1] / 2)
    cosPsi_2 = math.cos(euler[2] / 2)
    sinPsi_2 = math.sin(euler[2] / 2)
    quat = [0, 0, 0, 0]
    quat[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2
    quat[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2
    quat[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2
    quat[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2
    
    return quat

def matrix_product_vector_3(matrix:List[List[float]], vector:List[List[float]]) -> List[float]:
    """
    @Description : 三维矩阵与三维向量相乘
    @Param:
    @Returns     : 三维向量
    """
    result = [0.0, 0.0, 0.0]
    result[0] = matrix[0][0] * vector[0] + matrix[0][1] * vector[1] + matrix[0][2] * vector[2]
    result[1] = matrix[1][0] * vector[0] + matrix[1][1] * vector[1] + matrix[1][2] * vector[2]
    result[2] = matrix[2][0] * vector[0] + matrix[2][1] * vector[1] + matrix[2][2] * vector[2]

    return result

def quat_2_rotmax(quat: List[float]) -> List[float]:
    """
    @Description : 四元数转旋转矩阵
    @Param:
        quat: 四元数
        q0 q1 q2 q3
        w x y z
    @Returns     :
        R: 旋转矩阵
    """
    aSq = quat[0] * quat[0]
    bSq = quat[1] * quat[1]
    cSq = quat[2] * quat[2]
    dSq = quat[3] * quat[3]
    R = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
    R[0][0] = aSq + bSq - cSq - dSq
    R[0][1] = 2 * (quat[1] * quat[2] - quat[0] * quat[3])
    R[0][2] = 2 * (quat[0] * quat[2] + quat[1] * quat[3])
    R[1][0] = 2 * (quat[1] * quat[2] + quat[0] * quat[3])
    R[1][1] = aSq - bSq + cSq - dSq
    R[1][2] = 2 * (quat[2] * quat[3] - quat[0] * quat[1])
    R[2][0] = 2 * (quat[1] * quat[3] - quat[0] * quat[2])
    R[2][1] = 2 * (quat[0] * quat[1] + quat[2] * quat[3])
    R[2][2] = aSq - bSq - cSq + dSq
    return R

def rad_2_deg(rad:float):
    deg = rad * 180 / math.pi
    return deg

def deg_2_rad(deg:float):
    rad = deg * math.pi / 180
    return rad

def cov_m_2_lat_long_alt(ref:List[float], x:float, y:float, z:float) -> List[float]:
    """
    @Description : 笛卡尔坐标系转经纬度坐标系
    @Param:
        ref: 参考点经纬度
        x y z: 笛卡尔坐标系坐标
    """
    result = [0.0, 0.0, 0.0]
    if (x == 0 and y == 0):
        result[0] = x
        result[1] = y
    else:
        local_radius = math.cos(deg_2_rad(ref[0])) * EARTH_RADIUS

        result[0] = ref[0] + rad_2_deg(x / EARTH_RADIUS) # 得到的是lat, x为北向的位置
        result[1] = ref[1] + rad_2_deg(y / local_radius) # 得到的是long, y为东向的位置

    result[2] = ref[2] + z

def cov_lat_long_2_m(a_pos:List[float], b_pos:List[float]) -> List[float]:
    """
    @Description : 经纬度坐标系转笛卡尔坐标系
    @Param:
        a_pos: 参考点经纬度
        b_pos: 目标点经纬度
    @Returns     : 目标点相对于参考点的ned距离
    """
    lat1 = a_pos[0]
    lon1 = a_pos[1]

    lat2 = b_pos[0]
    lon2 = b_pos[1]
    
    n_distance = deg_2_rad(lat2 - lat1) * EARTH_RADIUS # 计算两个点之间的北向距离

    r_at_ref1 = math.cos(deg_2_rad(lat1)) * EARTH_RADIUS
    
    e_distance = deg_2_rad(lon2 - lon1) * r_at_ref1 # 计算两个点之间的东向距离,ned向东增加,经度向东减小

    m = [0.0, 0.0]
    m[0] = n_distance
    m[1] = e_distance

    return m
    